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ABSTRACT 


An  error  ellipse  plotting  routine  was  developed  to 
provide  real-time  indication  of  Kalman  filter  performance. 

The  study  included  an  evaluation  of  the  Hewlett-Packard  HP-86 
computer  system's  capability  for  providing  real-time 
tracking  information  and  an  evaluation  of  the  computer's 
possible  use  on  the  three-dimensional  underwater  tracking 
range  at  the  Naval  Underwater  Weapons  Engineering  Station, 
Keyport ,  Washington.  A  series  of  tracking  runs  were  used 
to  demonstrate  both  linear  and  extended  Kalman  filtering. 
Information  obtained  from  the  error  ellipses  was  used  to 
modify  filter  parameters  for  improved  filter  performance. 

It  was  found  that  the  error  ellipse  was  useful  as  a  tool 
for  indicating  filter  performance  and  for  making  decisions 
regarding  filter  parameter  modif ication.  The  HP-86  provided 
accurate,  reliable  results  and  it  could  be  used  for  on-line 
graphics.  However,  the  computing  speed  fo  the  HP-86  computer 
as  used  in  this  study  was  too  slow  for  on-line  processing 
of  the  three-dimensional  tracking  problem. 
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I.  INTRODUCTION 


The  Kalman  filter's  importance  as  an  estimator  and 
predictor  is  well  documented.  Providing  real-time  informa¬ 
tion  concerning  filter  performance  so  that  on-line  adjust¬ 
ments  to  filter  parameters  can  be  made  continues  to  be  an 
area  of  high  interest.  This  study  investigates  the 
usefulness  of  the  error  ellipse  as  a  tool  for  providing  a 
real-time  indication  of  filter  performance. 

Part  of  the  investigation  involves  an  evaluation  of  the 
Hewlett-Packard  HP-86  computer  system's  capacity  to  operate 
in  a  real  time  tracking  environment,  and  its  capabilities  for 
providing  information  concerning  filter  performance.  The 
rationale  behind  this  investigation  is  based  on  the  require¬ 
ment  for  the  Naval  Underwater  Weapons  Engineering  Station, 
Keyport,  Washington,  to  accurately  acoustically  track 
torpedoes  on  a  three-dimensional  underwater  tracking  range. 

A  knowledge  of  the  range  operation  is  not  within  the  scope 
of  this  study.  It  is  sufficient  to  know  that  presently 
the  range  receives  four  time  measurements  every  1.31  seconds, 
and  these  measurements  are  nonlinear  functions  of  the 
torpedo  position. 

To  gain  a  better  understanding  of  the  error  ellipse,  a 
4-state  tracking  scenario  was  chosen  for  this  study. 
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Initially,  the  linear  tracking  problem  is  discussed, 
followed  by  an  investigation  of  the  nonlinear  problem.  Of 
primary  interest  are  on-line  methods  to  improve  filter 
performance  using  information  provided  by  the  error  ellips 
for  filter  parameter  modification. 


II.  KALMAN  FILTER  THEORY 


A.  LINEAR  MATHEMATICAL  MODEL 


1.  The  Plant 


For  this  model  the  state  and  measurement  equations 
for  the  plant  are  linear.  Hence  the  discrete  form  is  used. 
The  assumed  plant  model  is  described  by  a  linear,  vector 
difference  equation: 


x(k+l)  =  <(>x(k)  +  Au(k)  +  rw(k)  (State  Equation)  (2-1) 


and  a  linear,  vector  measurement  equation: 


z(k)  =  cx(k)  +  v(k) 


(2-2) 


where : 


x(k)  is  an  n-dimensional  column  vector,  denoting  the 
state  of  the  plant  at  "time"  k. 

u(k)  is  the  deterministic  control  input,  an  m-vector, 
at  time  k. 

w(k)  is  a  p-dimensional  vector  representing  any  random 
forcing  inputs  at  time  k. 

z_(k)  is  a  q-dimensional  vector  representing  measure¬ 
ments  made  at  time  k. 

v(k)  is  a  q-dimensional  vector  representing  random 
measurement  made  at  time  k. 


£,  A,  r_,  and  c  are  assumed  constant  coefficient  matrices  of 
dimension  nxn,  nxm,  nxp ,  and  qxn  respectively. 
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2.  Noise  Processes 


In  order  to  place  probabilistic  structure  on  the 
noise  processes  v(k)  and  w(k)  the  following  assumptions  are  • 

made : 

(a)  v(k)  and  w(k)  are  individually  white  processes, 

that  is,  for  any  k  and  1,  with  k  i  1,  v(k)  and  • 

v(l)  are  independent  random  variables,  and  w(k) 
and  w(l)  are  independent  random  variables. 

(b)  v(k)  and  w(k)  are  individually  zero  mean,  • 

Gaussian  processes  with  known  covariances. 

(c)  v(k)  and  w(k)  are  independent  processes. 

Thus  for  the  measurement  noise:  • 


MEAN:  E[v(k) ]  =  0  (k= 0 , 1 , 2 , 3 , . . . )  (2-3) 

COVARIANCE:  E[v( k) vT( 1) ]  =  E[v(k) ]E[vJ(l)  ] 

=  0  k  t  1 

A 

=  Rk  k  =  1 

or  E[ v( k) vT  (1)]  =  Rk«kl  (k  ,  1= 0 , 1 , 2  ,  .  .  .  )  (2-4) 


where  5,  ,  is  the  Kronecker  delta  function  defined  as: 
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kl 


0,  k  i  1 
1,  k  =  1 
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Likewise  for  the  random  forcing  input: 


E  [  :  •?  ( k  )  ]  =  3 


( = n  i  > 

J  J  w  ^  ^  •  •  •  J 


(2-5) 


E[w( k)wT  (1)]  =  Qk  5kl 


k, 1  =  0, 1,2, 3,...) 


(2-6) 


Q,  and  R,  are  nonnegative  definite  symmetric  for  all 
*“K  K 

k.  Also  since  v(k)  and  w(k)  are  zero  mean  and  independent 
then : 


E [ v ( k ) w^  (1)]  =  0 


(2-7) 


For  the  purposes  of  this  study,  unless  otherwise 
specified  Qk  and  are  considered  to  be  known  and  constant, 
although  both  may  be  time  varying. 

3 .  Initial  State  Description 

For  the  initial  state  of  the  difference  equation  (2-1) 
it  is  unlikely  that  x^  will  be  available.  Hence,  it  is 

assumed  that  x  is  a  Gaussian  random  variable  of  known  mean 

— o 

x  and  known  covariance  P  ,  i.e., 

-o  -o’’ 


ECx(O)]  =  x 
—  — o 


EC[(x  -  x  )][(x  -  x  )]x}  =  P 

— o  — o  — o  — o  — o 
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This  choice  for  the  initial  state  has  the  advantage 
of  causing  the  subsequent  estimation  scheme  to  be  unbiased 
for  all  t.  [Ref.  1]  Further  it  is  assumed  that  the  initial 
state  and  the  measurement  noise  are  uncorrelated: 

E [ x ( 0 ) vT  (k)]  =  E [v ( k)x 1 ( 0 ) ]  =  0  (k=0 ,1 , 2 , 3  , .  .  . ) 

Also  the  initial  state  and  the  random  forcing  input  are 
uncorrelated : 

E[x( 0 ) wT  (k) ]  =  E[w(k)xT  (0)]  =  0  (k=0 , 1 , 2 , 3 , . . . ) 

B.  DISCRETE-TIME  ESTIMATION 

1 .  The  Estimator  Equations 

The  estimation  problem  involves  generating  an 
optimal  estimate  for  x  ( j  )  for  the  system  described  by  the 
difference  equation  (2-1)  from  the  noisy  measurements 

z( 0 )  ,z( 1 )  , .  . .  ,z( j )  .  This  estimate  will  be  denoted  by 

* 

x(j/j)  ,  which  means  the  estimate  of  x  at  time  j  given 
measurements  at  times  up  to  and  including  time  j .  The 
estimate  must  be  optimal  in  the  sense  that  the  expected 
value  of  the  sum  of  the  squares  of  the  error  in  the  estimate 
is  a  minimum,  i.e.: 


E{[(x(k/k)  -  x(k) ]T[x(k/k)  -  x(k)]} 


minimum 


The  estimator  is  characterized  by  the  linear  relationship: 


x(k/k)  =  x(k/k-l)  +  G(k)[z(k)  -  cx(k/k-l) ]  (k=C,l,:,..) 


(2-8) 


where 

A 

x(k/k)  is  the  optimal  (minimum  variance)  estimate  of 

x(k)  given  observations  at  times  up  to  and 
Tncl'uding  k. 

A 

x(k/k-l)  is  the  optimal  one-step  prediction  of  x(k) 
given  observations  at  times  up  to  and 
including  k-1. 

G(k)  is  the  optimal  estimation  gain  matrix  which 

will  minimize  the  variance  of  estimation 
error . 


For  the  initial  estimate  x(0/0),  the  estimator 

A 

equation  (2-8)  is  initialized  with  x(0/-l),  which  is  not  a 

A 

random  variable.  If  x(0/-l)  is  selected  such  that: 


x(0/-l)  =  E[x(0)]  =  x 

—  —  — o 

A 

it  can  be  shown  that  this  choice  of  x(0/-l)  makes  the 
estimator  unbiased  for  all  k.  [Ref.  1]  The  estimators  best 
available  information  concerning  x(k-l)  is  the  estimate 

A 

x(k-l/k-l) ,  therefore  it  is  reasonable  to  assume  that 

x(k/k-l)  =  <)>x(k-l/k-l)  +  Au(k-l)  (2-9) 


is  the  best  prediction. 


In  summary,  equations  (2-8)  and  (2-9)  are  the  estimator 

A 

equations,  with  x(0/-l)  =  x^  as  the  initial  condition. 

2 .  Gain  and  Covariance  Equations 

Without  going  into  detailed  derivations,  tne  optimal 
estimator  gains,  G(k),  used  in  the  estimator  equation  (2-8), 
are  those  which  satisfy: 


GOO  =  P(k/k-l)CT[CP(k/k-l)CT 


(2-10) 


P(k/k)  =  [I  -  G(k)C]P(k/k-l)  (2-11) 

P(k+l/k  J  =  $P(k/k)<{>T  +  Q  (2-12) 

with  the  initial  conditions: 

P(0/-1)  =  P  =  E{ Cx( 0 )  -  x_ ] Cx ( 0 )  -  x  ]T> 

—  — o  —  — o  —  — o 

where 

P(k/k)  =  E{[x(k/k)  -  x(k)][x(k/k)  -  x(k)]1} 


is  the  covariance  of  estimation  error  matrix. 


P(k/k-l)  =  E{[x(k/k-l)  -  x ( k ) ] [x( k/k-1)  -  x(k)]"} 

is  the  covariance  of  one-step  prediction  error  matrix. 

2  =  E[r_(k) -w(k) -wT  ( k)  *£T  (k)  3 

is  the  state  excitation  matrix. 

P(k/k)  and  P(k/k-l)  are  symmetric,  positive  definite  matrices. 

Several  observations  can  be  made  concerning  the  linear 
Kalman  gain  (2-10),  covariance  (2-11,  2-12)  and  estimator 
(2-8)  equations. 

(a)  The  estimator  gains,  G(k)  ,  do  not  depend  on  the 
measurement  data  and  hence  can  be  precomputed,  stored,  and 
used  as  the  processing  measurements  become  available. 

(b)  Although  not  obvious  from  the  equation,  the 
time-varying  gain,  G(k)  ,  depends  in  time  as: 

G(k)  =  fk-^»  [Ref.  2]  (2-13) 


» 


» 


i 


» 


Thus  the  effect  is  to  weight  the  correction  term, 

[^(k)  -  cx(k/k-D],  in  the  estimator  equation  (2-8)  less 
heavily  as  time  progresses.  The  advantage  of  a  greater 
initial  weight  allows  for  possibly  large  differences 
between  z(k)  and  x(k/k-l)  during  the  initial  observations, 
and  a  large  gain  will  result  in  a  significant  change  in  the 


» 


next  estimate.  This  advantage  is  also  borne  out  in  that 
there  is  less  confidence  in  the  quality  of  the  estimates 
during  the  early  observations  compared  with  the  quality 
after  numerous  observations.  Hence  the  later  an  observation 
the  less  drastic  an  estimate  will  be  altered  or  affected  by 
an  isolated  observation  discrepancy. 

(c)  In  general,  the  variance  of  estimation  error 
decreases  in  a  manner  analogous  to  the  gain  schedule  (2-13), 
i.e.,  it  decreases  as  k  grown  larger,  reflecting  greater 
confidence  in  the  estimate  as  the  number  of  observations 
increases.  Selection  of  the  proper  initial  condition,  P^, 
is  important  when  studying  the  effect  of  measurement  errors 
on  the  behavior  of  the  estimate.  So  should  be  assigned 
pessimistic  values  which  would  correspond  to  a  lack  of 
information  about  the  initial  state.  In  cases  when  the 
initial  state  is  completely  unknown,  then  £^-*“1.  [Ref.  3] 

(d)  The  Q  matrix  serves  to  compensate  for  model 
errors  and  prevents  the  covariance  matrix  from  becoming  too 
small  or  optimistic.  A  small  covariance  matrix  would  result 
in  a  small  filter  gain,  and  subsequent  observations  are 
essentially  ignored,  which  could  result  in  filter  divergence 
The  2.  matrix  prevents  G(k)  from  approaching  zero  by  adding 
uncertainty  to  the  system  which  is  reflected  in  a 
degradation  of  certainty  (increase  in  P(k+l/k)). 


C.  NONLINEAR  ESTIMATION  -  EXTENDED  KALMAN  FILTER 


In  many  practical  applications,  the  state  equations 
and/or  measurement  equations  are  nonlinear.  Before  the 
Kalman  filter  equations  can  be  used,  the  problem  must  be 
linearized  and  the  Kalman  filter  equations  are  applied  with 
some  modification. 

1 .  Nonlinear  Model 

Consider  a  nonlinear  discrete  system  of  state  and 
observation  equations  given  by: 

x(k+l)  =  f(x(k),  u(k) ,k)  +  w(k)  (2-14) 


and 


z(k)  =  h(x(k) ,  k)  +  v(k)  (2-15) 

In  these  equations  f  and  h  are  nonlinear  functions 
of  the  state  variable  x,  w(k)  is  the  plant  excitation  noise, 
and  v(k)  is  the  measurement  noise.  The  plant  noise  and 
measurement  noise  are  assumed  to  be  uncorrelated,  zero-mean, 
and  white.  The  same  equations  (2-3  thru  2-7)  apply  as  for 
the  linear  model. 

2 .  Extended  Kalman  Filter  Equations 

In  order  to  apply  the  linear  filter  equations, 
equations  (2-14)  and  (2-15)  are  expanded  about  the  best 
estimate  of  the  state  at  that  time  and  only  the  first-order 
terms  are  kept. 
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That  is,  defining  A(k)  as: 

3f 

A(k)  =  —  (x(k/k) ,  u(k),  k) 

—  a  X  —  — 

and 

3h 

H(k)  =  r —  (x(k/k-l) ) 

—  3  x  — 

As  can  be  seen  from  the  above  equations,  the  filter 

A  A 

estimates,  x(k/k)  and  x(k/k-l)  are  used  as  the  "best" 
estimates  about  which  the  linearization  is  performed.  The 
matrices  A(k)  and  H(k)  must  be  used  to  generate  G(k)  so  it 
is  available  to  process  z(k)  when  it  is  obtained.  The 
modified  extended  Kalman  filter  equations  are  then: 

Gain  Equation: 

G(k)  =  P(k/k-l)HT  (k)CH(k)  •  P(k/k-l )  •  HT  (k)  +  R]"1 

(2-16) 

Filter  Update  Equation: 

x(k/k)  =  x (k/k-1 )  +  G(k)[z(k)  -  h(x(k/k-l))]  (2-17) 
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Prediction  Equation: 


x(k+l/k)  =  f(x(k/k),  u(k),  k) 


Covariance  of  Estimation  Error  Equations: 


P(k/k-l)  =  A(k-l)P(k-l/k-l)AT(k-l)  +  Q(k-l) 
P(k/k)  =  Cl  -  G(k)H(k)]P(k/k-l) 


For  the  initial  estimate  x(0/0),  equation  (2-17)  is 

A 

initialized  with  x(0/-l)  with 


x(0/-l)  =  E[x(0)]  =  x 

—  —  — o 

A 

x(0/-l)  is  also  used  to  initially  evaluate  H(k). 
As  in  the  linear  case: 


PC0/-1)  =  P 


=  E[  (x  -  x  )  ]  C  ( x  -  x  i1] 
—0—0  — o  — o 


(  2-18  ) 

(2-19) 

(2-20) 
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III.  ERROR  ELLIPSOIDS 


A.  THEORY 

A 

Since  the  estimate  x(k/k)  is  unbiased  in  the  Kalman 

filter  equations,  the  P(k/k)  matrix  represents  the  covariance 

of  the  error  in  the  estimate.  If  the  estimate  were  biased, 

P(k/k)  would  represent  the  second-moment  matrix  rather  than 

the  covariance  matrix.  Hence,  P(k/k)  provides  significant 

information  about  the  accuracy  of  the  estimate.  If  the 

physical  model  is  accurately  described  by  the  state  and 

measurement  equations  (2-1,  2-2),  then  P(k/k)  can  be  used 

to  describe  the  manner  in  which  the  estimate  converges 

(or  diverges)  to  the  true  state.  Examination  of  the  P(k/k) 

matrix  directly,  element  by  element,  is  not  a  realistic 

2 

approach,  since  the  matrix  contains  n  elements,  where  n  is 
the  number  of  state  variables.  To  simplify  the  situation 
the  concept  of  the  error  ellipsoid  is  used.  [Ref.  4] 

As  discussed  earlier,  the  assumptions  are  made  that  the 
initial  state  of  the  plant  x^  is  Gaussian,  as  are  the  random 
processes  v(k)  and  w(k).  Using  these  assumptions  it  follows 

A 

that  x(k)  and  x(k/k)  are  also  Gaussian  since  they  are  linear 
combinations  of  Gaussian  variables  and  deterministic 
quantities.  Using  the  same  rationale,  the  estimation  error, 
defined  as: 
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e(k/k)  -  x(k/k)  -  x(k) 

is  also  Gaussian.  Using  the  fact  that  the  mean  of  the 
estimation  error  is  0,  the  probability  density  function  for 
e(k/k)  is: 

PeCe(k/k)]  =  C  (  2  Tr)n/  2  |  P(k/k)  j  1/2]“1 

exp[-l/2e2  (k/k ) P-^ (k/k) e (k/k) ]  (3-1) 

The  density  function,  pg[e(k/k)]  will  have  a  constant 

value  whenever  the  exponent  has  a  constant  value.  That  is: 

•  • 

-l/2eT  (k/k)P_1  ( k/k ) e (k/k )  =  c 

or 

eT  (k/k)P_1(k/k)e(k/k)  =  c2  (3-2) 

where  c  is  an  arbitrary  constant. 

As  demonstrated  by  Sorenson  [Ref.  1]  and  Kirk  [Ref.  2], 
it  can  be  shown  that  the  locus  of  points  e(k/k)  which 
satisfy  equation  (3-2)  are  hyperellipsoids.  For  the 
two-dimensional  case  which  is  of  concern,  equation  (3-2) 
describes  an  ellipse.  This  can  be  seen  by  fixing  time  and 
rewriting  (3-2)  as: 
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T  2 

e  we  =  c 


(3-3) 


wnere 


w  =  P  (k/k)  (a  2  x  2  symmetric  matrix) 


Expanding  the  left  side  of  (3-3)  gives: 


Wllel  +  w12w21ele2  +  w22e2  =  C 


which  because  of  symmetry  gives: 


wllel  +  2w12ele2  +  W22e2  "  C 


(3-4) 


Since  w^>  0 ,  w27>0,  and  w^  W22>W12’  ecluation  (3-4) 
describes  an  ellipse,  in  which  the  principal  axes  do  not 
coincide  with  the  coordinate  axes.  The  ellipse  is  rewritten 
in  terms  of  ^.(1)  and  £(2)  as  the  coordinate  axis,  and  it  can 
be  shown  that  y.(  1 )  and  y  ( 2 )  are  the  eigenvectors  of  w  with 
A ^  and  A ^  defined  as  the  corresponding  eigenvalues.  [Ref.  2] 
(See  Figure  3-1).  The  equation  for  the  ellipse  can  be 
rewritten  in  terms  of  a  coordinate  system  having  unit  vectors 
in  the  directions  of  ^(1)  and  ^(2)  as  the  basis  vectors. 


The  ellipse  equation  becomes: 

AlZ2(l)  +  A2y.2(2)  =  C2 


(3-5) 
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Figure  3-1  Error  Ellipse 


Remembering  that  w  =  P  ^(k/k),  it  can  be  shown  that  the 
corresponding  eigenvectors  and  eigenvalues  for  w-1=P(k/k) 
are  ^(1),  y (  2 )  ,  c^,  and  <*2  j  where  a;L=  —  and  a2  =  . 

Equation  (3-5)  can  be  rewritten: 


For  the  purposes  of  this  study,  the  term  error  ellipsoid 
refers  to  the  specific  case  when  c  =  1.  So  equation  *'?-•>} 
becomes : 

y  2  ( 1 )  y  2  (  2 ) 

-  +  -  =  i 

al  a  2 

In  terms  of  the  Cartesian  coordinates  xf,  yf,  which  use  e^ 
and  e2  as  basis  vectors: 

xf  =  xcosG  +  ysinQ 
y*  =  -xsin0  +  ycos0 

where  0  is  the  angle  of  rotation  of  the  axes  and  can  be 
computed  from: 

,  2cov(e  e  ) 

0  =  1/2  tan_i[  - -  ]  [Ref.  5] 

var(e  )  -  var(e  ) 
x  y 

For  a  given  value  of  c  it  is  possible  to  integrate  the 
probability  density  over  the  surface  of  the  error  ellipse  to 
obtain  the  probability  that  a  particular  sample  point  will 
lie  within  the  ellipsoid.  For  this  study,  n  =  2,  c  =  1, 
and  the  probability  the  error  is  inside  the  ellipse  is  0.394. 

In  summary,  the  error  ellipsoid  can  be  used  to 
characterize  the  concentration  of  the  estimate  about  the 
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true  value  of  the  state.  A  decrease  in  the  magnitude  of  an 
axis  of  the  ellipse  is  an  indication  that  the  error  in  the 
estimate  is  decreasing  in  that  direction. 

One  important  item  that  needs  to  be  pointed  out  is  that 
often  the  components  of  the  state  vector  represent  entirely 
different  types  of  variables,  for  example  the  components 
might  represent  range,  velocity,  and  depth.  Since  the  two- 
dimensional  ellipses  are  determined  by  using  two  components 
of  the  state  vector,  it  is  reasonable  to  examine  submatrices 
relating  state  variables  of  the  same  character.  Doing  so 
will  preclude  most  scaling  difficulties  when  plotting  the 
ellipses,  and  provide  more  meaningful  insight  in  to  the 
results . 

3.  ERROR  ELLIPSOIDS  AND  FILTER  DIVERGENCE 

Thus  far  the  discussion  has  centered  around  using 
submatrices  of  the  P(k/k)  covariance  of  estimation  error 
matrix  as  the  input  for  the  error  ellipse  to  indicate 
filter  performance.  As  proposed  by  Heffes  [Ref.  6]  and 
Nishimura  [Ref.  7],  P(k/k)  can  be  considered  as  a  "design" 
covariance  matrix  P  ,  using  the  assumption  that  the  only 
errors  are  in  Q,  ,  R.  ,  and  ?  with  the  following  inecualities 
holding  for  ail  k: 


(3-7) 


with  the  subscript  "d"  indicating  designed  and  "a"  indicating 

actual.  Equation  (3-7)  implies  more  input  noise,  more 

measurement  noise,  and  more  initial  state  uncertainty  in  the 

design  than  actually  exists.  This  conservative  filter  design 

results  in  a  somewhat  pessimistic  design  error  covariance 

Pd(k/k).  The  actual  error  covariance  Pa(k/k)  resulting 

d.  d  d 

from  using  a  filter  designed  with  Q  ,  R  ,  and  is  related 
to  the  design  error  covariance  in  the  following  manner: 

Pd(k/k)  >  Pa(k/k) 


This  result  is  particularly  useful  when  one  simply  does 

not  know  accurately  the  noise  covariance  of  the  input  or 

output,  but  an  upper  bound  is  known.  Designing  assuming 

the  noise  covariance  is  at  its  upper  bound  will  result  in 

Pa(k/k)  being  upper  bounded  by  PG(k/k).  In  some  sense  a 

worst  case  design  results.  Filter  divergence  exists  when 
.  .  d 

the  design  error  covariance  P  (k/k)  remains  bounded  while 
the  error  performance  matrix  ?a(k/k)  becomes  very  large 
relative  to  Pd(k/k)  or  is,  in  fact,  unbounded. 


IV. 


PROBLEM  DEFINITION 

A.  PROBLEM  DESIGN 

The  purpose  of  the  tracking  problem  is  to  study  the  use 
of  error  ellipsoids  as  real-time  indicators  of  filter 
performance.  In  order  to  keep  the  design  model  realistic 
albeit  reasonably  simplified  for  ease  of  study,  a 
two-dimensional  tracking  problem  using  several  different 
tracks  has  been  selected. 

1 .  Linear  Tracking 

All  tracks  are  based  on  an  x-y  coordinate  system 
with  the  target  moving  in  the  x  or  y  direction  relative  to 
the  sensor  located  at  the  origin.  Thus  for  aircraft 
tracking,  altitude  is  considered  constant,  as  is  depth  for 
torpedo  tracking. 

Defining  the  state  variables  as: 
x^  =  x  x-coordinate  of  the  target  location. 

=  x  velocity  of  target  ( v^ )  in  x-direction. 

x3  =  7  y-coordinate  of  the  target  location. 

x4  =  y  velocity  of  target  (v  )  in  y-direction. 


2T 


resulting  in  a  state  vector: 

x 

X 

X  = 

y 

_y 

The  following  are  the  state  equations: 
x^(t)  =  x2(t) 

x^Ct)  =  w-^Ct) 

x3<t)  =  x4(t) 

x4(t)  =  WjCt) 


(4-1) 


where  w^(t)  and  w^Ct)  are  assumed  to  be  uncorrelated, 
random  processes  that  account  for  unknown  target  accelerations 
and  nonlinear  target  motions.  Writing  the  discrete  form  of 
the  state  equations  gives: 
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I 

I 

I 


x.,(k+l)  =  x^(k)  +  T  •  x^(k)  +  ~  ’  w^(k) 
*2 Ck+1)  =  x ^ ( k )  +  T  •  w^(  k ) 

rn  2 

x^Ck+l)  =  x,(k)  +  T  *  x^(k)  +  •  w^(k) 

x4(k+l)  =  x4(k)  +  T  •  w2(k) 

or 


(4-3) 

I 


x(k+l)  =  fx(k)  +  rw(k) 


(4-4) 

I 


i 


With  T,  rhe  sampling  period  equal  to  1  second,  the  matrices 
4>  and  r  are : 


♦  = 


i — \ 

i 

i — 1 

0 

1 

o 

.5 

1 

o 

0 

i 

0 

0 

1 

0 

r 

— J 

II 

0 

0 

1 

r — 1 

j 

o 

.5 

1 

o 

0 

0 

1_ 

o 

1 

1_ 

(4-5) 


It  is  assumed  that  the  sensor  gives  noisy,  but 
uncorrelated  measurements  of  x  and  y.  Hence  the  discrete 
measurement  equations  are: 


i 

* 


2  3 


i 


z ,  (  k )  =  x  (  k )  +  v .,  (  k ) 

1  X  _L 

z2(k)  =  x^Ck)  +  v2(k) 


with  v^(k)  and  v^Ck)  uncorrelated  random  noise. 

Thus  for  the  measurement  equation: 

z(k)  =  cx(k)  +  v(k)  (4-7) 


the  matrix  c  is: 


> 


1  0 


c  = 


0 


0 


0  0 

1  0 


For  the  initial  run  and  unless  otherwise  noted  the 
following  values  for  the  Gaussian  random  processes  will  be 
used : 


E[v(k) ]  =  0  for  all  k 


E [ v ( k ) v  ^ ( k ) ]  = 


'20x10  0 


20x10' 


m  =  R  for  all  k 


» 


i 


» 


3  0 


► 


I 


m  =  the  standard  deviation  of 


measurement  noise, 


E[w(k)]  =  0  for  all  k 


100  0 


E[w(k)w  (k) ]  = 


(m/sec  )  =  cov  w  for  all  k 


m/sec  =  the  standard  deviations  of  the 
random  forcing  input. 


The  covariance  of  estimation  error  matrix  is  initialized: 


0  0 


?  =P(0/-1) 
— o  — 


10  0 


10  0 


0  0 


Since  the  filter  is  to  be  unbiased,  the  initializer ion : 


» 


x(0/-l)  =  x  =  Initial  condition  of  the  problem. 


2 .  Nonlinear  Tracking 

^  »  i 

ihe  state  equations  are  the  same  as  for  me  linear 
tracking  problem.  The  measurement  equation  is  considered 
as  a  noisy  range  measurement  by  the  tracking  sensor  and  is 

»  < 

characterized  as: 

z(k)  =  [x2(k)  +  x2(k)]1/2  +  v,(k)  (4-8) 

13  1 

»  < 

Thus  z(k)  is  a  nonlinear  function  of  the  states.  Using 

equation  (2-14)  and  with  T  =  1  second:  »  < 


f (x(k) ,  u(k) ,  k) 


x^(k)  +  x^(k) 
x2(k) 

x0(k)  +  x.  (k) 
_x4(k) 


*  4 


>  i 

} 

3  2  1 


►  ^ 


Taking  partial  derivatives  cf  f  with  respect  tc  x  civ-:: 


A(k) 

—  3x 


(x(k/k) ,  u(k) ,  k) 


using  equation  (2-15): 


h(x(k),  k)  =  [x^(k)  +  x^Ck)]"'"^ 


and  taking  the  partial  derivative  with  respect  to  x  gives 


H(k)  = 


(x(k/k-l) ) 


x^k) 

x^(k)+Xj(k)]1//^ 


x  3  ( k ) 

[x2(k)+x2(k) ]1/2 


x(k/k-l) 


> 


Using  the  above  results  with  the  sane  values  fur  me 
random  noise  processes  as  previously  stated  for  the  linear 
case,  the  extended  Kalman  filter  equations  can  new  oe  applied 
to  the  nonlinear  tracking  problem. 

B.  COMPUTER  SIMULATION 

1 .  Computer  Hardware/Software 
a.  Hardware 

All  computer  simulations  were  run  on  the  Hewlett- 
Packard  HP-86  personal  computer.  This  particular  model  was 
chosen  to  evaluate  its  capabilities  in  determining  its 
usefulness  in  actual  torpedo  tracking  at  the  underwater 
tracking  range  at  Naval  Underwater  Weapons  Engineering 
Station,  Keyport,  Washington.  The  KP-86  system  used  included 
keyboard,  9  inch  CRT  monitor  connected  through  an  integrated 
monitor  interface,  and  two  HP  Flexible  Disc  Drives  connected 
through  an  integrated  disc  interface.  Plotting  was  done  on 
a  HP-7225B  Plotter  and  printing  on  a  HP-2631G  Printer.  These 
peripherals  were  interfaced  using  a  HP-IB  Interface  module. 
Because  the  system  uses  interface  select  codes,  the  HP-13 
factory  preset  code  was  set  at  7 ,  which  is  the  select  code 
for  the  printer/disc  interface.  This  code  however  did  not 
work  when  interfacing  with  the  external  plotter  and  printer, 
since  duplicate  select  codes  are  not  allowed.  So  the 
internally  set  select  code  of  the  HP-IB  was  set  to  8  for 
proper  system  operation. 
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b.  Software 

The  HP-86  has  6JK  built-in,  useable  bytes  of 
computer  memory,  expandable  to  57 2K  using  either  32K,  54K, 
or  12 8K  Memory  Modules.  A  HP-86  plug-in  ROM  was  required 
to  operate  the  external  plotter.  Also  a  Matrix  ROM  was  used 
to  reduce  program  length  and  computer  run  time. 

All  programs  were  written  in  BASIC  programming 
language  using  REAL  (full)  precision,  which  provides  15  digit 
precision.  Appendix  B  provides  an  explanation  of  the  program 
options  and  Appendix  C  contains  the  program  listings  used  for 
this  study. 

2 .  Track  Generation 

To  evaluate  the  real-time  use  of  the  error  ellipse  as 
an  indicator  of  filter  performance,  Monte  Carlo  simulation 


runs  were  made.  Four  tracks  were  generated  by  separate 
programs  and  one  second  incremental  values  of  x,  y,  v  ,  and 
v  were  stored  in  data  files.  Appendix  I  contains  an 
explanation  of  the  generation  of  tracks  three  and  four. 

3  .  Noise  Generation 

In  order  to  simulate  the  random  noise  processes,  the 
computer's  random  number  generator  was  used  and  the  generated 
numbers  scaled  accordingly.  For  each  track  and  each 
different  value  of  noise  sigma,  a  different  generator  "seed 
number"  was  used.  These  noise  values  produced  were  added  to 
the  aDDlicable  true  track  values  to  simulate  a  sensor 


measurement  corrupted  by  independent  Gaussian  noise.  For  all 


3  3 


» 


1 


l 


» 


> 


cases  where  filter  parameters  were  varied  for  a  particular 
track  under  a  specific  noise  condition,  one  noisy  trac i:  was 
generated,  stored,  and  used  throughout  that  oarticular 
simulation.  This  was  done  for  ease  of  filter  performance 


comparison . 


4.  Gating  Scheme 


In  order  to  preclude  catastrophic  filter  failure  due 


to  excessive  measurement  noise,  a  bound  was  established  for 
the  maximum  acceptable  limits  of  measurement  noise.  A 
three-sigma  gate  was  designed  using  the  covariance  of  the 
measurement  noise,  R,  and  the  predicted  covariance  of  error 
matrix  P(k/k-l) .  The  gate  is  defined  as: 


Gate ( k)  =  3(p..  (k/k-1)  +  R..)1/2 

11  ,  11 

max 


This  gate  is  the  maximum  error  allowable  for  the  measurement 
at  time  k.  If  the  absolute  difference  between  the  actual 
measurement  received  and  the  predicted  measurement  is 
greater  than  the  three-sigma  gate,  then  that  particular 
measurement  data  is  rejected  as  unacceptable.  When  this 
occurs,  the  filter  gain,  G(k),  is  set  to  zero,  resulting  in 
that  measurement  being  ignored  and  the  prediction  of  the 
states  set  equal  to  the  estimate,  that  is: 


x(k/k)  =  x(k/k+l) 
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5 .  Collection  of  Statistics 

In  order  to  study  error  ellipses  as  an  indicator  of 
filter  performance,  statistics  were  calculated,  or.  line, 
after  each  measurement  during  the  Monte  Carlo  run.  The 
statistics  computed  were  relative  error  (in  some  cases  the 
error  was  normalized),  error  mean,  error  variance,  and  error 
covariance  for  the  positional  variables.  The  following 
equations  apply: 


Relative  Error  =  e(k/k)  =  x(k)  -  x(k/k) 


(filter  error  residual) 

k 

Error  Mean  =  e(k/k)  -  1/k  Z  e(j/j) 

j=l 


_  2 

Error  Variance  =  Var[e.(k/k)]  =  1/k  Z  [e . ( j / j ) ] -[ e . (k/k) ] 

1  j=l  1  1 
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V .  TARGET  TRACKING  AMD  ERROR  ELLIPSE  ANALYSIS 

A.  LINEAR  TRACKING 

Track  1  depicts  a  target  approaching  at  a  constant 
velocity  of  223.6  feet  per  second.  The  solid  line  of  Figure 
5.1  indicates  the  true  track  of  the  target  and  the  numbers 
along  the  track  indicate  the  time  in  seconds.  The  target 

was  tracked  in  a  measurement  noise,  a  =  150,  with  the 

5  -v 

random  forcing  noise  a  =1.  R  was  set  for  20,000.  The 

— w  — 

dots  indicate  the  filtered  track  using  the  linear  Kalman 

filter  equations.  Figures  5.2  and  5.3  are  the  filter  error 

ellipses  for  this  run,  computed  at  increments  of  10  seconds. 

As  can  be  seen  the  ellipse  size  decreases  with  increasing 

time  indicating  filter  convergence.  The  computed  ellipse 

surface  areas  shown  on  the  figures  confirm  this.  Figure  5.4 

shows  the  filtered  track  for  the  case  where  o  has  been 

—v 

increased  to  300  and  all  other  parameters  remain  the  same. 

The  error  ellipses  of  Figure  5.5  computed  for  13-second 
increments  show  increasing  area  indicating  filter  divergence. 
Figure  5.6  shows  the  error  ellipses  for  the  same  track  run 
but  this  time  the  ellipses  were  computed  using  a  "statistics 
window"  of  10.  By  this  is  meant  that  the  ellipses  were 
derived  from  statistics  computed  for  the  last  10  data 
measurements.  All  previous  data  is  disregarded.  Using  this 


method,  the  ellipses  of  Figure  5.6  show  filter  convergence 
from  iterations  15  to  25.  The  filtered  track  of  Figor:- 
5.-i  confirms  this.  Figure  5.7  shows  the  results  tor  the 
same  track  parameters,  except  in  this  case  a  statistics 
window  of  5  was  used.  The  window  5  ellipse  area  at  time 
2  5  (  517  3  s g  ft)  is  much  less  than  the  area  of  either  the 
run  with  the  statistic  window  of  10  (11,540  sq  ft)  or  the 
run  with  no  window  at  all  (65,500  sq  ft).  This  is  expected 
since  the  filter  is  essentially  "locked  on"  at  time  20, 
and  the  window  5  ellipse  at  time  25  disregarae  all  data 
previous  to  time  20.  Figure  5.8  shows  the  error  ellipses 
for  the  same  track  but  the  measurement  noise  was  increased 
to  a  =  40C ,  while  R  was  kept  at  20,000.  The  error  ellipses 
indicate  filter  divergence,  and  indeed  the  filtered  track 
headed  off  in  the  wrong  direction. 

Track  2  depicts  a  target  approaching  at  a  constant  speed 
of  500  feet  per  second  in  the  -y  direction.  Figure  5.9 
depicts  the  solid  line  track  and  the  dots  indicate  the  linear 
filtered  track  with  J  =  150,  o  =  1  and  R  =  23,000.  Figure 
5.10  and  5.11  are  the  error  ellipses  for  the  run.  No 
statistics  window  was  used.  Other  than  the  fact  that  the 
ellipse  area  is  decreasing,  the  shape  of  the  ellipse  provides 
little  additional  infcrmaticn.  Figure  5.12  and  5.13  snow  the 
ellipses  for  the  normalised  error.  With  the  same  measurement 
noise  sigma  for  both  the  x-position  and  y-position  measuremen 
the  normalized  error  elliose's  share  and  orientation  reflect 
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the  target  track  proximity  to  either  axis.  In  Figure  5.12 
the  ellipse  major  axis  indicates  a  large  normalized  error  in 
tne  x-uirectaon.  inis  as  to  be  eroe  o  o  s_nce  ■  .~ii  o  \ 
maintains  a  constant  x-position  of  500  feet,  while  the 
y-position  is  initially  20,000  feet.  However,  near  time  40 
as  the  target  approaches  the  x-axis,  the  normalized  y  error 
increases  and  becomes  so  large  at  the  x-axis  crossing  that 
the  normalized  x  error  becomes  insignificant  in  comparison. 
This  can  be  seen  in  Figure  5.13  for  the  ellipses  at  time 
45  and  55  compared  with  the  ellipse  at  time  35.  The 
normalized  error  ellipse  is  an  excellent  indicator  of 
target  proximity  to  an  axis,  but  the  rapid  shifts  in  ellipse 
surface  area  make  it  difficult  to  determine  filter 
convergence . 

Track  3  depicts  a  target  approaching  on  a  parabolic 
track  at  a  speed  of  200  feet  per  second.  Figure  5.14  is  the 
true  track,  with  a  linearly  filtered  track  indicated  by  the 
dots.  For  this  run  a  =150,  a  =1-,  and  R=20,000.  Figure  5.15 
are  the  error  ellipse  plots  for  times  40,  50,  and  60,  the 
period  of  the  highest  rate  of  change  in  x-  and  y-velocity. 

The  ellipse  areas  increase  with  time  indicating  divergence. 
Figure  5,16  and  5.17  are  the  ellipse  plots  using  a  10-data 
point  and  a  5-data  point  statistics  window  respectively. 

In  both  cases  the  ellipse  areas  for  time  60  are  less  than 
time  50  indicating  the  filter  has  tracked  around  the  curve. 
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Using  error  ellipses  based  on  a  statistics  window  in  this 
tracking  situation  provides  a  better  indicator  of  filter 
performance. 

For  the  second  run  of  track  3,  a,,  was  increased  to  300 
and  all  other  parameters  remained  the  same.  Figure  5.18 
shows  the  resulting  filtered  track ,  which  obviously  didn't 
track  around  the  curve.  With  the  large  amount  of  measurement 
noise  and  considering  that  the  maximum  random  forcing  input, 
i.e.  the  maximum  acceleration  in  the  x-  and  y-direction, 
occurs  between  times  40  and  60,  it  is  a  logical  place  to  lose 
track.  Another  factor  to  be  considered  is  the  decrease  in 
gain  as  k  increases.  By  time  40  the  gains  have  little 
influence.  So  a  system  was  incorporated  in  the  program  to 
"reinitialize"  the  filter  by  setting  the  gains  ro  G(0),  if 
certain  conditions  were  met.  After  several  trial  and  error 
runs,  it  was  determined  that  if  a  statistics  window  of  10 
were  used,  and  the  gains  reinitialized  if  the  error  ellipse 
area  increased  consecutively  a  certain  number  of  times, 
that  the  filtered  track  would  follow  around  the  curve. 

Figure  5.19  is  the  filtered  track  using  a  statistics  window 
of  10  and  reinitializing  the  filter  if  the  error  ellipse  area 
increased  consecutively  during  five  1-second  increments. 

The  error  ellipses  for  that  run  are  shown  in  Figures  5.20 
and  5.21.  As  indicated  on  the  figures,  the  filter  was 
reinitialized  four  times  and  the  ellipse  areas  for  the 
10-second  increments  increased,  until  reinitializing  the 
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filter  at  time  52  locked  the  filter  in.  Consequently  the 
error  ellipse  areas  for  time  50  and  70  decreased,  indicating 
convergence . 

In  the  final  run,  the  filter  was  reinitialized  after  10 
consecutive  1-second  error  ellipse  increases,  with  all  other 
parameters  remaining  the  same.  Figure  5.22  is  the  filtered 
track;  Figures  5.23  and  5.24  are  the  error  ellipse  plots 
for  this  run.  As  indicated  on  Figure  5.24  the  filter  was 
reinitialized  only  once  at  time  57.  A  comparison  of  ellipse 
areas  for  the  last  two  runs  shows  that,  with  the  exception 
of  time  70,  the  areas  were  larger  in  the  first  run  when  the 
filter  was  reinitialized  4  times.  This  is  expected  since 
reinitializing  results  in  larger  gains  producing  more  widely 
vary  estimates,  and  hence  greater  error  variance.  At  time 
70  the  error  ellipse  area  of  the  first  run  is  less  since  in 
the  first  run  the  last  filter  reinitialization  occurred  at 
time  52  versus  time  57  in  the  second  run.  The  first 
filtered  track  had  more  time  to  settle  out  by  time  70, 
resulting  in  less  error. 

B.  MC1IL INEAR  TRACKING 

Track  4  depicts  a  target  moving  at  a  constant  velocity 
of  50  feet  per  second  (30  kts )  in  the  x-direction  for  15 
seconds,  at  which  time  the  target  turns  and  travels  in  the 
-y-direction.  (See  Figure  5.25)  Using  the  extended  Kalman 
filter  with  a  =30  and  R=900,  a  series  of  tracking  runs  were 
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made  for  various  values  of  COVW  (o  =from  20  to  200).  In  none 

w 

of  these  runs  did  the  filter  successfully  track  the  turn¬ 
around  the  turn.  Figure  5.25  shows  the  results  for  the  case 
when  C0VW=I50.  In  this  instance  the  filter  lost  track  as  the 
target  came  out  of  the  turn.  Trying  to  track  through  a  turn 
using  a  constant  COVW  (and  hence,  a  constant  Q)  did  not  work. 
So  a  scheme  was  devised  to  vary  COVW  dependent  on  information 
derived  from  the  error  ellipse.  After  several  trial  runs  for 
this  particular  track,  it  was  determined  that  if  the  error 
ellipse  area  increased  consecutively  for  7  iterations  of 
k,  COVW  would  be  doubled,  and  if  the  ellipse  area  decreased 
consecutively  for  5  iterations  of  k,  COVW  would  be  halved. 
Initially  the  the  trial  runs  were  made  without  a  statistics 
window,  and  the  filter  did  not  track  successfully.  Without 
the  statistics  window,  the  old  data  weighted  down  the  statis¬ 
tics,  and  the  error  ellipses  were  not  indicative  of  what  was 
currently  happening.  So  it  was  decided  to  use  a  statistics 
window.  Windows  of  5,  10,  and  15  were  tried.  Window  5 
proved  to  be  too  responsive  and  window  15  not  responsive 
enough.  So  a  statistics  window  of  10  was  chosen  for  the 
tracking  run.  With  39^=10^,  o_v~3Q,  R=900  ,  and  COVW  initially 
set  at  20,  the  tracking  run  was  made.  Figure  5.27  depicts 
the  filtered  track  output,  and  Figures  5.28-5.30  are  the 
10-second  incremental  error  ellipses  for  the  run.  As 
can  be  seen,  the  filter  did  track  around  the  curve.  Figure 
5.29  shows  the  ellipse  areas  are  becoming  less  between  times 
55  and  65.  Also  indicated  below  the  plots  are  the  values  of 
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COVW  for  the  k  time  the  plot  was  computed.  During  This 
particular  run  COVW  varied  from  an  initial  value  of  20  up 
to  160,  and  then  decreased  to  40  by  the  end  of  the  run. 

Using  the  same  filter  parameters  as  above  except  a_.^  was 
increased  to  200,  and  R  to  40,000,  another  run  was  made.  The 
filter  did  not  track  at  all.  During  this  run  COVW  varied 
from  an  initial  value  of  20  up  to  40  and  decreased  to  5  by 
the  end  of  the  run.  Obviously,  the  criteria  for  increasing 
and  decreasing  COVW  was  not  effective.  More  trial  runs  were 
necessary  to  determine  the  optimum  consecutive  increases 
or  decreases  of  the  ellipse  areas  before  adjusting  COVW 
accordingly . 

A  second  approach  to  the  nonlinear  tracking  problem,  one 
that  was  used  earlier  for  the  linear  case,  is  to  reinitialize 
the  filter  if  certain  conditions  are  met.  Again,  using 
trail  and  error  runs  with  and  without  statistics  windows, 
it  was  determined  that  using  a  statistics  window  of  10  gave 

the  best  results.  Using  initial  conditions  of  COVW-150, 

2  .... 

P  =10  ,  a  =30,  and  R=900  ,  several  runs  were  made,  reimtializ- 

ing  the  filter  if  the  error  ellipse  area  increased  consecu¬ 
tively  for  a  certain  number  of  iterations  of  k.  Of  the  runs 
attempted,  the  best  results  were  obtained  when  the  filter  was 
reinitialized  if  the  area  increased  for  5  consecutive 
iterations  of  k.  Figure  5.31  is  the  filtered  track  for  this 
run,  and  Figures  5  .  32-5  .  34  are  the  error  ellipse  plots. 
Indicated  below  the  plots  are  the  times,  k,  when  the  filter 

4U 


was  reinitialized.  For  this  particular  run  the  filter  was 

reinitialized  5  times.  It  should  be  noted  than  when 

reinitializing  the  filter,  ?(k/k-l)  was  reset  to  10 Z  x  ?  . 

was  not  large  enough  to  be  effective  in  getting  the 

2 

filter  back  on  track.  The  initial  value  of  10  was  used  for 

to  reflect  the  high  confidence  in  the  initial  state 

conditions.  Other  values  of  P  did  not  work  as  well. 

— o 

Using  the  same  parameters  as  above  except  the  filter  was 
reinitialized  after  7  consecutive  error  ellipse  area 
increases,  another  run  was  made  with  the  resulting  track 
depicted  in  Figure  5.35.  A  comparison  with  Figure  5.31 
reveals  that  using  7  area  increases  as  the  criterion  for 
filter  reinitialization  resulted  in  poorer  filter  performanc 
as  can  be  shown  from  the  error  ellipses. 

The  final  nonlinear  filter  tracking  runs  attempted 
involved  simultaneously  varying  COVW  and  filter  reinitializa 
tion.  The  results  were  disastrous,  and  highly  unpredictable 
It  was  an  interesting  experiment  in  futility,  and  no 
meaningful  results  could  be  obtained. 
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A.  ERROR  ELLIPSE 


The  filter  error  ellipse  proved  useful  as  a 
indicating  filter  performance.  The  information 
the  ellipse,  particularly  surface  area  changes, 
make  decisions  concerning  the  alteration  of  the 
parameters.  Several  approaches  for  using  the  e 
were  applied  to  both  the  linear  and  nonlinear  t 
Droblen  and  the  results  are  summarized  below: 


tool  for 
.  provided 
was  used 
filter 
rror  ellip 
racking 


by 

t  o 


s  <2 


Procedure  Applicable  Comments 
Filter 
(Linear  or 
Extended) 


Statistics  5oth 
Window 


useful  in  keeping  the  error 
ellipse  current  and  responsive 
to  present  data.  The  ellipse 
reflects  most  recent  data;  old 
data  is  disregarded.  Beneficial 
when  making  a  decision  concernin 
filter  parameter  modification. 
Normally  a  better  indicator  of 
filter  convergence  or  divergence 
than  without  a  statistics  window 


Normalized 

Error 


Aid  in  displaying  error  trends 
as  target  approaches  coordinate 
axis  or  origin.  Not  practical 
in  determining  filter  conver¬ 
gence/divergence  due  to  rapid 


Both 


Reinitialise  Linear-  Use  increasing  ellipse  sice  =s  an 


and  P(k+i/K)  have  settles  cue. 

Mere  effective  when  used  in 
conjunction  with  statistics  window. 


This  technique  of  varying  CO VW 
based  on  error  ellipse  area  in¬ 
crease  or  decrease  is  particularly 
useful  in  a  tracking  environment 
containing  large  variations  in  the 
random  forcing  input.  The  proce¬ 
dure  is  more  effective  when  used 
in  conjunction  with  a  statistics 
window.  Using  this  technique 
with  filter  reinitialization  was 
unsuccessful . 


B.  COMPUTER  PERFORMANCE 

The  HP-86  proved  tc  be  an  extremely  reliable  computer 
with  no  downtime  experienced  during  the  5  month  period  of 
operation.  Fuli(Real)  precision  was  used  throughout  the 
study  providing  15  digit  precision,  which  was  more  than 
adequate.  The  use  of  the  Matrix  ROM  reduced  program  length 
by  90d  and  increased  computing  speed  by  a  factor  of  about  6. 
Although  not  used  in  this  study,  a  Statistics  ROM  would  have 
undoubtedly  further  increased  computing  speed.  For  any 
further  study  using  the  HP-36,  it  is  recommended  that  a 
Statistics  ROM  be  procured. 

It  took  approximately  2  seconds  for  each  incremental 
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equations,  both  for  the  iinea 


Error  Extended 

r  q  1  2.  r  0  e 

Expansion/ 

Compression 
to  vary  COVW 
(Adaptive  Q) 


and  nonlinea 


computing  time  also  included  all  statistics  computations. 
With  an  incoming  data  rate  of  1  set  cf  measurement  data  car 
second,  this  computing  speed  is  not  sufficient  for  on-line 
processing.  As  previously  mentioned,  the  Naval  Underwater 
Tracking  Range  receives  a  series  of  4  measurement  times 
sequentially  every  1.31  seconds.  The  range’s  three- 
dimensional  tracking  problem  will  necessarily  involve  more 
than  the  4  state  variables  used  in  this  study.  Hence 
greater  matrix  dimensions  resulting  in  longer  computing 
times  can  be  expected. 

The  HP-86  CRT  graphics  were  used  extensively  to  provide 
error  ellipse  plots  during  the  tracking  runs.  Using  a  "no 
frills"  approach  to  plotting,  i.e.  plotting  without  x-y 
axis  or  labelling,  it  took  approximately  2.5  seconds  per 
ellipse  plot.  The  ellipse  plotting  routine  used  involved 
sines  and  cosines,  plotted  point  by  point  in  30  degree 
increments  for  a  total  of  360  degrees.  This  method  was 
somewhat  slow.  Had  there  been  available  a  graphics  program 
that  would  sketch  in  the  ellipse  around  the  intersected 
major  and  minor  axis,  the  graphics  presentation  could  have 
been  speeded  up.  But  since  ellipse  plotting  for  every  3  to 
5  increments  of  time  provided  sufficient  "real-time” 
information,  the  time  of  2.5  seconds  per  plot  was  tolerable. 

Summarizing,  the  HP-35  could  be  used  to  compute 
statistics  and  provide  graohics  in  the  real-time  underwater 


rracking  environment,  if  the  graphics  were  required  not  more 
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However,  before  the  HP-35  ca 


often  than  3  to  5  seconds, 
considered  feasible  for  real-tine  Hainan  filter  process! 


:igat  to: 


;ed ad  an  fir.-; 


computer  processing  tine  such  as  parallel  processing, 
additional  use  of  manufacturer-provided  ROMs  and  machine 


language  programming. 
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APPENDIX  A 
TRACK  GENERATION 

1.  TRACK  THREE 

Target  movement  is  in  the  x-y  plane  with  the  tracking 
sensor  located  at  the  origin  of  the  cartesian  axes.  The 
target  follows  a  parabolic  track  (see  Figure  A-l)  at  a 
constant  speed  of  200  feet  per  second. 


l 
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The  parabolic  equation  is: 


y*"  =  4p ( x-h ) 


where  p  =  1000  and  h  =  1000,  resulting  in: 
y2  =  400  0 (x-100  0 ) 

Initial  target  location  is  (x,y)  =  (8000,  5291.5).  Target 
x-direction  velocity  is  given  by: 


v  =  v  cos (Angle) 


( A-l ) 


and  target  y-direction  velocity  is: 


v^  =  v  sin(Angle) 


(A-  2 ) 


where  the  Angle  is  obtained  from: 


Angle  =  tan 


Ax 


and  v  is  obtained  from: 


,  2  2,1/2 

V  =  ( V  +  V  ) 

x  y 
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where  the  argument  of  the  inverse  tangent  is  the  slope  of  a 
small  increment  (less  than  1  second)  at  each  successive  data 
point.  Using  a  sampling  period  of  one  second,  the  data 
points  (x,y)  can  be  obtained  from: 

x(k  +  l)  =  x(k)  +  v_^  ( k ) 

y(k+l)  =  y(k)  +  v^(k) 

Table  A-l  gives  the  numerical  values  for  the  four  states. 

2.  TRACK  FOUR 

Target  movement  simulates  a  30-knot  torpedo  (velocity  50 
feet  per  second)  at  a  constant  depth.  The  target's  initial 
position  is  (x,y)  =  (3250,  5000),  and  it  is  moving  in  the  v 
direction  with  v  =0.  (See  Figure  A-2.)  The  target  remains 
on  a  straight  course  for  15  seconds,  and  then  executes  a  90 
degree  turn  and  travels  in  the  -y  direction  at  v^=  -50  ft/sec. 
The  trajectory  of  the  target  turn  is  described  by  a  90  degree 
arc  of  a  circle  of  radius,  R  =  1000,  with  the  circle  centered 
at  (x,y)  =  (4000,  4000).  The  90  deg  arc  will  be  traversed 
in: 

2irR/4v  sec.  =  IQtt  sec. (where  v  =  50  ft/sec) 
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TABLE  A-l 


I 

I 


I 


NUMERICAL  VALUES  OF  STATES  FOR  TRACK  THREE 


K 

1 

2 

3 

•4 

e 

6 

7 

8 
9 

10 
1 1 
12 

13 

14 

15 

16 

17 

18 
IS 
20 
21 
22 

23 

24 

25 

26 

27 

28 
2S 

30 

31 

32 

33 

34 

35 

36 

37 

38 

3  S 
40 

4  1 

42 

43 

44 

45 


X 

X-VEL 

eooo. co 

-186.90 

7813.  1C 

-  186. 9  3 

7626 .17 

-186 .60 

7439. 5E 

-  186. 25 

7253.33 

-185. 89 

7067.44 

-  185. 5  1 

6881.93 

-185 . 1 1 

6696. 82 

-  184. 6  8 

6512.14 

-184.2  4 

6327.90 

-  183. 76 

6144.14 

-183. 26 

5960. 87 

-  182.  73 

5778.  14 

-  182.  17 

5595.98 

-  181.  57 

5414.41 

-180.92 

5233. 49 

-  180. 24 

5C53.25 

-179.51 

4873. 74 

-  178.  72 

4695. G2 

-177.87 

4517.  15 

-176. 96 

4340.  19 

-175. 97 

4164. 22 

-174.91 

3989.31 

-  173 .74 

3815.57 

-  172.  48 

3643.09 

-171 . 09 

3472. 00 

-  169. 57 

3302.43 

-167.89 

3134.54 

-  166 . 04 

2968. 5C 

-163. 98 

2804. 52 

-  161. 67 

2642. 85 

-159.09 

2483. 76 

-  156.  17 

2327. 59 

-152.86 

2174. 74 

-  149. 07 

2C25.66 

-144 . 72 

1880. 95 

-  139. 67 

1741 . 28 

-133.78 

1607. 50 

-  126.  83 

1480.67 

-118.59 

1362.  08 

-108.70 

1253.38 

-96. 73 

1156. 65 

-82. 0  1 

1C74. 64 

-63.44 

1011. 20 

-37. 24 

1000. CO 

0.00 

Y 

Y-VEL 

5291 . 50 

-71.15 

5220. 38 

-71.13 

5148.  27 

-71.98 

5075*  26 

-72.87 

5001.  33 

-73.79 

4926.  43 

-74.74 

4850. 54 

-75.73 

4773. 60 

-76.76 

4695. 59 

-77.93 

4616. 45 

-78.94 

4  5  36.  14 

-80.09 

4454. 60 

-81.30 

4371 . 79 

-82.56 

4287. 65 

-83.87 

4202.  10 

-85.24 

4115. 09 

-86.68 

4026. 54 

-88.19 

3936. 37 

-89.78 

3844. 49 

-91.44 

3750. 81 

-93.19 

3655. 24 

-95.04 

3557. 65 

-97.00 

3457. 93 

-99.06 

3355. S3 

-101.25 

3251. 52 

-103.57 

3144. 52 

-106.05 

3034. 75 

-108.68 

2922. 01 

-111.50 

2806.  06 

-1 14.51 

2686. 65 

-1 17.74 

2563. 47 

-121.21 

2436. 20 

-124.94 

2304. 42 

-128.98 

2167. 70 

-133.33 

2025. 50 

-1 38.05 

1877.  18 

-143.15 

1721 . 95 

-148.67 

1558. 85 

-154.64 

1386. 61 

-161.05 

1203.  46 

-167.88 

1006 . 73 

-175.05 

791. 58 

-182.41 

546.  4  1 

-189.67 

211.63 

-196.50 

0.  00 

-200.00 
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TABLE  A-l  (COHT.) 


K 

X 

X-VEL 

V 

X 

Y-VEL 

46 

1011. 20 

10. 57 

-211.63 

-199.72 

47 

1C21 .77 

25.  14 

-295. 07 

-198.41 

48 

1046. 90 

35. 8  2 

-433.  13 

-196.77 

45 

1C82.72 

48 . 8  9 

-575. 23 

-193.93 

5C 

1 131 . 6  1 

61.85 

-7254  57 

-190.20 

51 

1  193.46 

74.4  9 

-879. 69 

-185.61 

52 

1267. 55 

86. 36 

-1035. 28 

-1 8C.39 

53 

1354. 31 

97.25 

-1190.  48 

-174.77 

54 

145 1 . 56 

107.0  4 

-1343. 96 

-168.94 

5  5 

1558. 6  C 

115.75 

-1494.  80 

-163.  10 

56 

1674. 35 

123. 4  3 

-1642. 38 

-157.37 

57 

1797.78 

130.  1  6 

-1786. 37 

-151. 35 

58 

1527.94 

136.  0  6 

-1926. 60 

-14b.  58 

59 

2C64.01 

141  .24 

-2063. 01 

-141.61 

60 

2  20  5  •  24 

145. 78 

-2195. 67 

-136.92 

61 

2351 .02 

149. 78 

-2324. 67 

-132.54 

6  2 

2500. 80 

153. 31 

-2450.  14 

-128.43 

6  3 

2654.  1  1 

156.45 

-2572. 25 

-124.60 

64 

2810. 56 

159. 24 

-2691.  14 

-121.01 

65 

2969. 8C 

161 . 73 

-2806.  99 

-1  17.66 

66 

3131. 5  2 

163.  97 

-2919. 95 

-114.52 

67 

3295.45 

165.98 

-3030.  17 

-111.58 

68 

3461. 47 

167. 80 

-3137.  61 

-108.32 

68 

3629.27 

169.46 

-3243.  01 

-106.23 

70 

3 798. 73 

170. 96 

-3345.  68 

-103.79 

71 

3569.65 

172.34 

-3446. 56 

-101 . 49 

72 

4  142.  03 

173. 6C 

-3545.  15 

-  99.32 

73 

4315.63 

174.76 

-3641 . 77 

-97.26 

74 

4490. 38 

175.  8  2 

-3736. 51 

-95.32 

75 

4666.21 

176 . 8  1 

-3829. 47 

-93.48 

76 

4843. 02 

177.  73 

-3920. 72 

-91  .73 

77 

5020.74 

178.57 

-4010.  36 

-90.06 

78 

5  199. 32 

179.  36 

-4098.  45 

-88.48 

79 

5378. 68 

180 .  1  0 

-4185.  06 

-86.97 

80 

5558. 78 

180. 79 

-4270.  26 

-85.53 

e  i 

5739.57 

181  .  44 

-4354.  1  1 

-84.15 

82 

5521. 0  1 

182. 04 

-4436. 67 

-82.33 

83 

6103.05 

182 . 61 

-4517.99 

-81.57 

84 

6285.66 

183.  14 

-4598.  1  1 

-80.36 

85 

6468. 80 

183 .65 

-4677. 09 

-79.20 

86 

6652.45 

184.  13 

-4754. 98 

-78.09 

87 

6636.58 

184 . 58 

-4831 .  80 

-77.01 

88 

7021.  16 

185.00 

-4907. 61 

-75.98 

85 

7206.  16 

185. 4  1 

-4982. 43 

-74.99 

90 

7391 . 57 

185. 79 

-5056.  31 

-74.03 

Figure  A-2  Track  4 


And  the  equation  for  a  circle: 


9 


It  follows  that  the  arc  of  Figure  A-2  is  described  by: 


x(k)  =  4000  +  1000sin( . 05k) 
y(k)  =  4000  +  lOOOcos ( . 0 5k) 


where  the  angle  argument  is  in  radians  and  k=0 , 1 , 2 , . . . 3 1 

seconds.  The  velocities  v  and  v  can  be  obtained  from: 

x  y 


v  Ck)  =  50cos( .05k) 
x 

« 

v  (k)  =  -S0sin( .05k) 

y 

using  the  same  angle  argument. 

The  track  values  for  the  track  arc  are  contained  in  Table 

A-2. 


» 


» 


91 


T  A3LE  A-2 


( 


NUilESIt'AL  VALUES  CF  STATES  EOR  TRACK  FOUR 


K 

X 

X- VEL 

Y 

Y-VEL 

12 

3049  .9  5 

49. 9  8 

4999. 38 

-  1. 25 

13 

3099.56 

49.94 

4997. 50 

-2.  50 

14 

3149.66 

49. 8  6 

4994. 28 

-3.75 

15 

3199.67 

49. 75 

4990. 01 

-4.  99 

16 

2249 . 3b 

49 . 6  1 

4984. 40 

-6.  23 

17 

3298.e8 

49.44 

4977. 54 

-7.  47 

18 

334  8 . 2  2 

49.  24 

4969. 45 

-8.71 

IS 

3397.34 

49 .00 

4960.  13 

-9.  93 

20 

3446 . 2  1 

48.74 

4949.  5  S 

-11.16 

21 

3494 .61 

48.4  5 

4937.  82 

-12. 37 

22 

254  3  .C  6 

48.  1  2 

4924. 65 

-13.53 

23 

2591  .  C4 

47.77 

4910. 67 

-14.  73 

24 

3638.62 

47.  3  8 

4895. 30 

-15. 97 

25 

3685. 8C 

46  .  9  7 

4878. 75 

-17.14 

26 

3732.55 

46.  53 

4861. 02 

-18.31 

27 

3778 . 64 

46 . 05 

4842.  12 

-19.47 

28 

3824.64 

45. 55 

4822. C8 

-20.62 

25 

3869. S3 

45.02 

4800.  89 

-21. 75 

30 

3914.68 

44.  46 

4778.  5  S 

-22.87 

31 

3958.65 

43 .88 

4755.  17 

-23. 97 

32 

4002.43 

43. 2  7 

4730. 65 

-25.06 

33 

4045 . 37 

42.  63 

4705. 05 

-26.  13 

34 

4087.67 

41.96 

4678. 38 

-27.  19 

35 

4129.26 

41.27 

4650. 67 

-28. 23 

36 

4170.  19 

40 . 5  5 

4621.  S3 

-29. 25 

37 

4210.37 

39.80 

4592. 17 

-30.26 

38 

4249 .75 

39.  0  4 

4561.  4  1 

-31.24 

2  9 

4288 . 44 

38.24 

4529. 68 

-32.21 

40 

4326.27 

37. 4  2 

4497. CO 

-33.  16 

4  1 

4363 .28 

36.58 

4463. 38 

-34. 08 

42 

4399 . 43 

35.  7  2 

4428. 84 

-34.99 

43 

4434 .71 

34 . 84 

4393. 41 

-35.  87 

44 

4469.  10 

33.  93 

4357.  11 

-36. 73 

45 

4502.56 

33 .00 

43 19. 97 

-37.56 

46 

4535. C9 

32.  05 

4  281  .  S 9 

-38. 38 

47 

4566 . 65 

31  .08 

4243.  22 

-.'9.17 

•  4 


•  4 


•  4 


•  4 


•  4 


•  i 


A- 2  (C cm.) 


K 

X 

X-VEL 

Y 

Y-VZL 

48 

4597. 24 

30.0  9 

4203. 67 

-39.93 

49 

4626. 83 

29 . 08 

4163.  37 

-40.67 

50 

4655. 4C 

23. 0  6 

4122. 34 

-41  .39 

51 

4632 .54 

27.0  2 

4080. 60 

-42.07 

52 

4709.43 

25. 9  5 

4038. 20 

-42.74 

53 

4734.65 

24.88 

3995.  14 

-43.37 

54 

4759.  18 

23.  79 

3951.  46 

-43.98 

c  c 

4782.41 

22.68 

3907.  19 

-44.56 

56 

4804. 54 

21 . 56 

3862. 35 

-45.1  1 

57 

4825 . 53 

20.4  2 

3316.  97 

-45.64 

58 

4845. 38 

19.  2  8 

3771. 09 

-46.13 

55 

4664  .  C8 

18.  12 

3724. 72 

-46.60 

60 

4881.61 

16.  95 

3677.  69 

-47.04 

61 

4897.57 

15. 77 

3630. 64 

-47.45 

62 

4913.  14 

14.  58 

3583. CO 

-47.83 

63 

4927. 12 

13.37 

3535. 00 

-48.1  8 

64 

4939.65 

12.  17 

3486.  66 

-48.50 

€5 

4951 . 45 

10.95 

3438. 01 

-48.79 

66 

4961.75 

9.73 

3389.  10 

-49.04 

67 

4970. 5C 

8.50 

3335. 93 

-49.27 

6  8 

4978 . 78 

7.  26 

3290. 56 

-49.47 

65 

4985.43 

6.03 

3241 . 01 

-49.64 

70 

4990. 63 

4.  76 

3191. 30 

-49.77 

71 

4  99  4 .55 

3.54 

3141.  47 

-49.87 

72 

4997.90 

2.  29 

3091. 56 

-49.95 

73 

4999.57 

1.04 

30U1 .  59 

-49 .9  9 

74 

4999 . 58 

-.2  1 

2991.  59 

-50.00 

AFPEMTIX  3 

COMFUTER  PROGRAM  EXPLANATION 


1 .  LINKAL 

The  LINKAL  program  computes  the  filter  salts,  GAIN  (4,2), 
for  the  4-state  system  and  stores  the  gains  in  "LINGAIM 
. STORAG".  The  theoretical  covariance  of  error  matrix,  ?1L\(4,4), 
is  also  computed  and  stored  in  "LINCOV . STORAG" .  Several 
different  sets  of  gain  and  covariance  values  were  computed 
and  stored  for  various  values  of  measurement  noise,  RMAT ( 2 , 2 )  , 
and  random  forcing  noise,  C07W(2,2). 

2 .  LIMES? 

The  LIMEST  program  retrieves  the  appropriate  gain 
schedule  from  storage  and  computes  the  optimal  estimate, 
XHAT(4,i),  and  the  optimal  one-step  prediction,  XHK1X(4,1). 

The  following  capabilities  are  contained  in  the  LIMEST  program: 

a.  Gating  Scheme 

If  the  absolute  difference  (TIFF)  between  the  one-step 
prediction,  XKKi'<  and  the  noisy  track  value,  ZMAT,  is  greater 
than  the  three-sigma  gate,  then  the  GAIN  matrix  is  disregarded 
and  XHAT  is  set  equal  to  XHK1X . 

b .  Track  Noise 

By  setting  MOITPAX  =  i,  the  random  number  generator, 

P.MD,  and  the  resulting  simulated  noise  produced,  71  and  72, 

i  u. 


are  bvcassed  and  the  track  values  corrupted  with  noi: 


JUli  .\A..  ::  _  , 


are  retrieved  from  data  me  ’  i 

then  the  random  number  generator  is  "reseeded"  s  or  eacn 
program  run,  producing  a  different  set  of  noise  values 
resulting  in  a  unique  noise  corrupted  track  for  each  run. 

c.  Statistics  Window 


When  WINDOW  is  set  to  0,  the  filter  state  statistics 
are  computed  after  each  Monte  Carlo  run.  The  error  mean, 
variance  and  position  covariance  are  computed.  If  WINDOW 
is  set  to  an  integer,  I,  such  that  max  k>I>0,  the  statistics 
will  be  computed  based  on  the  data  compiled  during  the  last 
I  iterations  of  the  simulation.  If,  for  example,  WINDOW=10, 
the  computation  of  statistics  will  be  based  on  the  data 
obtained  during  the  last  ten  iterations  of  k,  and  all 
previous  data  is  disregarded, 

d.  Error  Normalization 


By  setting  NORM  =  1,  the  error  (ERR),  which  is 
defined  as  the  difference  between  the  true  track  value  (IRAK) 
and  the  estimate  (XHAT) ,  is  normalized.  For  all  other  values 
of  NORM  the  relative  error  is  used  in  computing  the 
statistics . 

e.  Reinitializing  Gains 


The  orogram  has  the  ootion  cf  reinitializing 


me 


to  G(Q).  This  can  be  done  by  setting  HIGH  to  an  inters 
such  that  max  .<>1-  0.  If  the  surface  area  of  the  ell  ir  r 
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increases  I  consecutive  times,  indicating  filter  divergence, 
the  gains  are  reinitialized  to  G(0).  If  reinitializing 
gains  is  not  desired  as  an  option,  then  HIGH  is  set  to  some 
arbitrary  large  number  greater  than  max  k. 

3 .  EXTKF 

The  EXTKF  program  computes  the  optimal  estimate,  XHAT , 
and  the  optimal  one-step  prediction,  XHK1K,  for  the  4-state 
nonlinear  tracking  problem.  The  EXTKF  program  has  the 
options:  gating  scheme,  track  noise,  statistics  window, 

and  error  normalization  as  described  for  the  LINEST  program. 
EXTKF  has  the  following  additional  options: 

a .  Reinitializing  the  Filter 

The  program  has  the  option  of  reinitializing  the 

covariance  of  one-step  prediction  error  matrix,  PK1K,  by 

setting  it  to  10nxPo,  where  n  is  zero  or  some  small  integer. 

If  HIGH  is  set  to  an  integer  I,  such  that  max  k>I>0,  and  the 

surface  area  of  the  ellipse  increases  I  consecutive  times, 

then  the  PK1K  matrix  will  be  reset  to  10nxP  . 

— o 

b .  Adaptive  Q 

EXTKF  has  the  option  of  automatically  increasing  or 
decreasing  the  state  excitation  matrix,  Q.,  under  certain 
conditions  by  changing  the  value  of  the  covariance  of 
excitation  noise,  COVW.  INCREASE  and  DECREASE  are  set  to 
integers  I  and  J  respectively,  such  that  max  k>I,J>0.  If 
the  surface  area  of  the  error  ellipse  increases  I 
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consecutive  times,  COVW  is  doubled  or  increased  by  some 
facxor.  Conversely,  if  the  surface  area  of  the  error  ellipse 
decreases  J  consecutive  times,  COVW  is  halved  or  decreased 
by  some  factor.  If  COVW  is  changed,  increased  for  example, 
the  value  of  COVW  can  be  changed  again  later  in  the  run, 
if  the  criteria  for  either  increasing  or  decreasing  is  met. 

If  the  adaptive  Q  is  not  desired  then  INCREASE  and  DECREASE 
are  set  to  some  large  number  greater  than  k. 
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APPENDIX  C 
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